/*
 * ManipulatorSystem.cpp
 *
 *  Created on: Jan 13, 2011
 *      Author: Robotics
 */

#include "ManipulatorSystem.h"
#include "DriverStation.h"
#include "CoordinateMath.h"
#include "Math.h"
//TODO: test The following for this file and MyRobot.cpp
/*
 * Test the Glove Joint calibration System
 * Test the new setpoint calculation system (the one based on Glove joint Max/Min values)
 * 
 * Test the Save and Load system for Calibration values from disk
 * 
 * Document
 */

#define SAVEFILE "/tmp/calibrations"


ManipulatorSystem::ManipulatorSystem():
m_clawPosition(0,18,12),
m_Victor1(5),
m_Victor2(6),
m_AnalogChannel1(1),
m_AnalogChannel2(2),
m_PID1(0.015, 0.0, 0.035, &m_AnalogChannel1, &m_Victor1, 0.05),
m_PID2(-0.02, 0.0, 0.01, &m_AnalogChannel2, &m_Victor2, 0.05),
m_Valve(8,1,2)

{

//PID (p, i, d, source, output, period = 0.05)
	
	mptr_DriverStation = DriverStation::GetInstance();
	m_Valve.Set(DoubleSolenoid::kForward);
	//Robot arm joint 1
	m_J1Min = 440.0;
	m_J1Max = 939.0;
	m_J1M = 180 / (m_J1Max - m_J1Min);
	m_J1Y = - m_J1M * m_J1Min;
	//Robot arm joint 2
	m_J2Min = 832.0;
	m_J2Max = 440.0;
	m_J2M = 290 / (m_J2Max - m_J2Min);
	m_J2Y = - m_J2M * m_J2Min;
	//Glove joint 1
	m_GJ1Min = 0.80;
	m_GJ1Max = 3.0;
	//Glove joint 2
	m_GJ2Min = 0.1;
	m_GJ2Max = 2.05;
	
	//reloadCalibration();
}

void ManipulatorSystem::reloadCalibration(){
	//TODO: TEST: Attempt to load the Calibration values from disk.
	float JointCalibrations[8] = {0,0,0,0,0,0,0,0}; 
	FILE* calFilePTR = fopen(SAVEFILE,"r");
	if(calFilePTR){
		int read = fread(JointCalibrations, sizeof(JointCalibrations[0]),8, calFilePTR);
		fclose(calFilePTR);
		if (read == 8){
			m_J1Min = JointCalibrations[0];
			m_J1Max = JointCalibrations[1];
			m_J1M = 180 / (m_J1Max - m_J1Min);
			m_J1Y = - m_J1M * m_J1Min;
			//Robot arm joint 2
			m_J2Min = JointCalibrations[2];
			m_J2Max = JointCalibrations[3];
			m_J2M = 290 / (m_J2Max - m_J2Min);
			m_J2Y = - m_J2M * m_J2Min;
			//Glove joint 1
			m_GJ1Min = JointCalibrations[4];
			m_GJ1Max = JointCalibrations[5];
			//Glove joint 2
			m_GJ2Min = JointCalibrations[6];
			m_GJ2Max = JointCalibrations[7];
		}
	}
}



ManipulatorSystem::~ManipulatorSystem()
{

}


/*RUN THIS DURING AUTONOMOUS.
 *Peg Height = the row which you want to score at ie; top"1", middle"2", low"3"
 *Peg Position = the column it will score on, ie; middle"1", outside"2".
 */
void ManipulatorSystem::Autonomous(int pegheight, int pegposition)
{



}

/*Grabs the code from the "glove"
 *to control the manipulator on the actual robot
 *
 */
void ManipulatorSystem::DriverControlGlove()
{

	//m_PID1->SetInputRange();
	//m_PID2->SetInputRange();
	
	if (m_previousDigitalIn == false && mptr_DriverStation->GetDigitalIn(1) == true)
	{	
		m_Valve.Set((m_Valve.Get()==DoubleSolenoid::kReverse) ? DoubleSolenoid::kForward: DoubleSolenoid::kReverse);		
	}
	m_previousDigitalIn = mptr_DriverStation->GetDigitalIn(1);
	
	//float DSJ1 = 228*(mptr_DriverStation->GetAnalogIn(2)-1.65) + (m_J1Max + m_J1Min)/2;
	float dGlove = m_GJ1Max+m_GJ1Min;
	float dBot = m_J1Max + m_J1Min;
	float DSJ1 = (dBot/dGlove)*(mptr_DriverStation->GetAnalogIn(2)-m_GJ1Min) + m_J1Min;

		if(DSJ1 < m_J1Min){
			DSJ1 = m_J1Min;
		}
		if(DSJ1 > m_J1Max){
			DSJ1 = m_J1Max;
		}
	m_PID1.SetSetpoint(DSJ1);
	m_PID1.SetOutputRange(-0.75f,0.75f);
	
	m_PID1.Enable();

	
	//float DSJ2 = -200*(mptr_DriverStation->GetAnalogIn(1)-1.65) + (m_J2Max + m_J2Min)/2;
	dGlove = m_GJ2Max-m_GJ2Min;
	dBot = m_J2Max - m_J2Min;
	float DSJ2 = (dBot/dGlove)*(mptr_DriverStation->GetAnalogIn(1)-m_GJ2Min) + m_J2Min;
	//min used to be 139
	//printf("DSJ2 before atten:  %f    ", DSJ2);
	if(DSJ2 > m_J2Min){
		DSJ2 = m_J2Min;
	}
	//max used to be 571
	if(DSJ2 < m_J2Max){
		DSJ2 = m_J2Max;
	}
	m_PID2.SetSetpoint(DSJ2);
	m_PID2.SetOutputRange(-1.0f, 1.0f);
	
	m_PID2.Enable();

	//printf("%f__",m_PID1.GetError()-m_preverr);
	m_preverr = m_PID1.GetError();

	//printf("analog in: %f, setpoint: %f, pot on arm: %f \n",mptr_DriverStation->GetAnalogIn(1), DSJ2, m_AnalogChannel2.PIDGet());
	//printf("joint1 position: %f , joint2 position: %f\n", m_AnalogChannel1.PIDGet(), m_AnalogChannel2.PIDGet());


	
	

	
}

/*Grabs the code from the joystick
 *to control the manipulator on the actual robot
 *
 */
void ManipulatorSystem::DriverControlJoystick(KHSJoystick* joystick1)
{

	if (m_previousDigitalIn == false && joystick1->GetTrigger() == true)
	{
		m_Valve.Set((m_Valve.Get()==DoubleSolenoid::kReverse) ? DoubleSolenoid::kForward: DoubleSolenoid::kReverse);		
	}
	m_previousDigitalIn = joystick1->GetTrigger();
		
	float j1 = joystick1->GetX();
	float j2 = joystick1->GetY();
	m_Victor2.Set(j2);
	m_Victor1.Set(j1);

	printf("\n");
	//printf("joint2 angle %f\n", Joint2PotToAngle());
	//printf("joint1 point: %f , joint2 position: %f\n", m_AnalogChannel1.PIDGet(), m_AnalogChannel2.PIDGet());
	//printf("pot1: %f, pot2: %f \n", m_AnalogChannel1.PIDGet(), m_AnalogChannel2.PIDGet());


	
	m_PID1.Disable();
	m_PID2.Disable();
	
}



/* Converts a number given by the pot. and converts to
 * an angle that the first arm is at.
 *
 */

double ManipulatorSystem::Joint1PotToAngle()
{
// change 1/3.2 to actual gear ratio if it isn't already	
 //return m_J1M*(m_AnalogChannel1.GetAverageValue())+m_J1Y;
	return m_AnalogChannel1.PIDGet();

}

double ManipulatorSystem::Joint1AngleToPot(float angle)
{
	return (1/m_J1M)*angle+(m_J1Y/m_J1M);
}



/* Converts a number given by the pot. and converts to
 * an angle that the second arm is at.
 *
 */
//Angle Potentiometer
//15    110
//180   362
//310   571
double ManipulatorSystem::Joint2PotToAngle()
{
	 //return m_J2M*(m_AnalogChannel2.GetAverageValue())+m_J2Y;
	return m_AnalogChannel2.PIDGet();
	
}

double ManipulatorSystem::Joint2AngleToPot(float angle)
{
	return (1/m_J2M)*angle+(m_J2Y/m_J2M);
}

Point3D ManipulatorSystem::GetCameraCoordinates()
{
	float x = 0;
	float y = 7+25.0*sin(r(Joint1PotToAngle()))+24*cos(r(270-Joint1PotToAngle()-Joint2PotToAngle()));		
	float z = 51-25.0
	*cos(r(Joint1PotToAngle()))+24*sin(r(270-Joint1PotToAngle()-Joint2PotToAngle()));

m_clawPosition.moveTo(x,y,z);
	
return m_clawPosition;
	
	
	
}

bool ManipulatorSystem::setArmJoints(float firstJoint, float secondJoint){
	m_PID1.SetSetpoint(firstJoint);
	m_PID2.SetSetpoint(secondJoint);
	m_PID1.Enable();
	m_PID2.Enable();
	return m_PID1.OnTarget() && m_PID2.OnTarget();
}
	
void ManipulatorSystem::setClamped(bool clamp){
	if(clamp){
		m_Valve.Set(DoubleSolenoid::kForward);
	}
	else{
		m_Valve.Set(DoubleSolenoid::kReverse);
	}
}

//TODO: Test Calibration (Btn 6,7, 10,11)
// Potential watchdog issues with blocking here.
/* Calibrate
 * This will set the Maximum and Minimum potentiometer readings for the Bot arm dynamically
 * These readings are used in many places and are critical to proper arm functioning.
 * The Calibration should be done with the following values:
 * JOINT1: 	minimum with arm vertical facing downward
 * 			maximum with arm vertical facing upward
 * JOINT2: 	minimum with secondary arm down and 55 degrees from the main arm
 * 			maximum with secondary arm up in starting position 15 degrees from main arm
 * 
 */
void ManipulatorSystem::calibrate(KHSJoystick* ptr_joystick){
	bool set = false;
	printf("======YOU ARE IN ARM CALIBRATION MODE========\n");
	printf("Move the Bot arm into position using the joystick.\n");
	printf("Then press the appropriate button to set the value\n");
	printf("Button 7: Joint 1 minimum\n");
	printf("Button 6: Joint 1 maximum\n");
	printf("Button 11: Joint 2 minimum\n");
	printf("Button 10: Joint 2 maximum\n");
	while (!set){
		DriverControlJoystick(ptr_joystick);
		if(ptr_joystick->GetDebouncedButton(7)){
			m_J1Min = m_AnalogChannel1.GetAverageValue();
			set = true;
		}
		if(ptr_joystick->GetDebouncedButton(6)){
			m_J1Max = m_AnalogChannel1.GetAverageValue();
			set = true;
		}
		if(ptr_joystick->GetDebouncedButton(10)){
			m_J2Max = m_AnalogChannel2.GetAverageValue();
			set = true;
		}
		if(ptr_joystick->GetDebouncedButton(11)){
			m_J2Min = m_AnalogChannel2.GetAverageValue();
			set = true;
		}
	}
	//TODO: TEST: Save the values to disk
	float JointCalibrations[8] = {m_J1Min, m_J1Max, m_J2Min, m_J2Max, m_GJ1Max, m_GJ1Min, m_GJ2Max, m_GJ2Min};
	FILE* calFilePTR = fopen(SAVEFILE,"w");
	if(calFilePTR){
		int written = fwrite(JointCalibrations, sizeof(JointCalibrations[0]),8,calFilePTR);
		fclose(calFilePTR);
		if (written == 8){
			reloadCalibration();
		}
	}
	printf("======YOU HAVE LEFT ARM CALIBRATION MODE========\n");
	
}

//TODO: Test Arm Calibration (Btn 8)
//Potential watchdog issues with blocking here
void ManipulatorSystem::calibrateArm(KHSJoystick* ptr_joystick){
	m_PID1.Disable();
	m_PID2.Disable();
	m_Victor2.Set(0);
	m_Victor1.Set(0);
	int calibration_stage = 1;
	printf("======YOU ARE IN GLOVE CALIBRATION MODE========\n");
	printf("Follow the instructions on screen.\n");
	printf("Go to Glove Joint 1 Maximum and hit Joystick button 8.\n");
	while(calibration_stage <5){
		if(ptr_joystick->GetDebouncedButton(8)){
			switch(calibration_stage){
				case 1:
					m_GJ1Max = mptr_DriverStation->GetAnalogIn(2);
					printf("Go to Glove Joint 1 Minimum and hit Joystick button 8.\n");
					break;
				case 2:
					m_GJ1Min = mptr_DriverStation->GetAnalogIn(2);
					printf("Go to Glove Joint 2 Maximum and hit Joystick button 8.\n");
					break;
				case 3:
					m_GJ2Max = mptr_DriverStation->GetAnalogIn(1);
					printf("Go to Glove Joint 2 Minimum and hit Joystick button 8.\n");
					break;
				case 4:
					m_GJ2Min = mptr_DriverStation->GetAnalogIn(1);
					break;
			}
			calibration_stage++;
		}
	}
	//TODO: TEST: save the Calibration Values to disk.
	float JointCalibrations[8] = {m_J1Min, m_J1Max, m_J2Min, m_J2Max, m_GJ1Max, m_GJ1Min, m_GJ2Max, m_GJ2Min};
	FILE* calFilePTR = fopen(SAVEFILE,"w");
	if(calFilePTR){
		int written = fwrite(JointCalibrations, sizeof(JointCalibrations[0]),8,calFilePTR);
		fclose(calFilePTR);
		if (written == 8){
			reloadCalibration();
		}
	}
	printf("======YOU HAVE LEFT GLOVE CALIBRATION MODE========\n");
}

void ManipulatorSystem::disable(){
	m_PID1.Disable();
	m_PID2.Disable();
}




